﻿#include "gait_math.h"
#include "eso.h"
#include "locomotion_header.h"
#include "include.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#if RUN_PI
#include "PositionVelocityEstimator.h"
#endif

Gait_Double_SLIP gait_rolling;

char rolling_way = 0;
char rolling_diag_way = 0;
char rolling_lsm = 0;
void  Gait_Rolling_Active(char way_sel)
{
	char i = 0;
	vmc_all.param.robot_mode = M_ROLLING;
	vmc_all.gait_mode = ROLLING;
	vmc_all.tar_att[YAWr] = vmc_all.att_ctrl[YAWr];
	vmc_all.tar_pos.z = vmc_all.pos_n.z;
	vmc_all.trot_state = 0;

	rolling_way = way_sel;
	if (rolling_way==Ls)
		rolling_diag_way = Rs;
	else
		rolling_diag_way = Ls;
	rolling_lsm = 0;

	robotwb.exp_att.yaw = robotwb.now_att.yaw;
	for (i = 0; i < 4; i++) {
		vmc[i].tar_sita1 = vmc[i].sita1;
		vmc[i].tar_sita2 = vmc[i].sita2;
		vmc[i].tar_sita3 = vmc[i].sita3;
		vmc[i].ground = 1;
		vmc[i].st_phase = 0;
		reset_tar_pos(i);
	}

	gait_rolling.slip_param.first_bound = 0;
	gait_rolling.slip_param.mess = Mw;
	gait_rolling.slip_param.c = 0.7;
	gait_rolling.slip_param.exp_z = 0.76*fabs(MAX_Z);
	gait_rolling.slip_param.T_sw = 0.22;
	gait_rolling.slip_param.state_all = 0;
	gait_rolling.slip_param.s_peak_Fz = 0.5;
	gait_rolling.slip_param.L = 0.57*Hw;
	gait_rolling.slip_param.vd = 0;

	gait_rolling.slip_param.k_time = 0.35;
	gait_rolling.slip_param.tauP = 15;//Nm/rad rolling前馈

	for (i = 0; i < 2; i++) {
		gait_rolling.slip[i].T_st_now = gait_rolling.slip[i].T_st_last = 0.35;

		gait_rolling.slip[i].T_sw = gait_rolling.slip_param.T_sw;
		gait_rolling.slip[i].T = gait_rolling.slip_param.T_sw + gait_rolling.slip[i].T_st_now;

		gait_rolling.slip[i].t = gait_rolling.slip[i].s = 0;
		gait_rolling.slip[i].time_fly_near = 0;
		gait_rolling.slip[i].g_st = 0;
		gait_rolling.slip[i].ground = 0;

		gait_rolling.slip[i].hip_n_exp.z = gait_rolling.slip_param.exp_z;

		gait_rolling.slip[i].pid_hip[0] = 2200;//高度
		gait_rolling.slip[i].pid_hip[2] = 20;

		gait_rolling.slip[i].pid_hip_x[0] = 150;//高度
		gait_rolling.slip[i].pid_hip_x[2] = 0;

		gait_rolling.slip[i].pid_v[0] = 50;//速度
		gait_rolling.slip[i].pid_v[1] = 10;//速度

		gait_rolling.slip[i].pid_R[0] = 35;//俯仰角
		gait_rolling.slip[i].pid_R[2] = 2;
		gait_rolling.slip[i].k_jump = 1;

		gait_rolling.slip[i].dhip_n_exp.x = 0;

		vmc_robot_p.ground_mu = 0.5;

		gait_rolling.slip[i].g_st = 1;

		gait_rolling.slip[i].T_air = (gait_rolling.slip[i].T_sw - gait_rolling.slip[i].T_st_now) / 2;

		gait_rolling.slip[i].T_st_now_trig = gait_rolling.slip[i].T_st_now;//触发式
		gait_rolling.slip[i].T_tirg = gait_rolling.slip[i].T;
	}
	gait_rolling.slip[Ls].hip_n_exp.z = gait_rolling.slip_param.exp_z;
	gait_rolling.slip[Rs].hip_n_exp.z = gait_rolling.slip_param.exp_z;

	gait_rolling.slip[Rs].phase = Rs;
	gait_rolling.slip[Ls].phase = Ls;
	gait_rolling.slip[Rs].other_phase = Ls;
	gait_rolling.slip[Ls].other_phase = Rs;

	gait_rolling.slip[Rs].ground = 1;//后腿先跳
	gait_rolling.slip[Ls].ground = 1;

	gait_rolling.slip_param.first_bound = 0;

#if RUN_WEBOTS||RUN_PI
	if (way_sel==Rs)
		printf("Tort Rolling Initing::Right!!!!!!!\n");
	else
		printf("Tort Rolling Initing::Left !!!!!!!\n");
#endif

	reset_servo_interge();
#if RUN_PI
	kf_pos_vel.rst_kf = 1;
	kf_pos_vel.force_ground = 0;
	moco8_ml();//reset param
#endif
}

void rolling_sensor_process(float dt)
{
	int i;

	for (i = 0; i < 2; i++) {
		gait_rolling.slip[i].att_now[PITr] = vmc_all.att[PITr];
		gait_rolling.slip[i].att_now_rate[PITr] = vmc_all.att_rate[PITr];
		gait_rolling.slip[i].att_exp[PITr] = -vmc_all.ground_att_cmd[PITr];

		gait_rolling.slip[i].att_now[ROLr] = vmc_all.att[ROLr];
		gait_rolling.slip[i].att_now_rate[ROLr] = vmc_all.att_rate[ROLr];
		gait_rolling.slip[i].att_exp[ROLr] = -vmc_all.ground_att_cmd[ROLr];
	}

	gait_rolling.slip[Ls].hip_n_now.x = (vmc[2].epos_n.x + vmc[3].epos_n.x) / 2;
	gait_rolling.slip[Ls].hip_n_now.y = (vmc[2].epos_n.y + vmc[3].epos_n.y) / 2;
	gait_rolling.slip[Ls].hip_n_now.z = (vmc[2].epos_n.z + vmc[3].epos_n.z) / 2;

	gait_rolling.slip[Rs].hip_n_now.x = (vmc[0].epos_n.x + vmc[1].epos_n.x) / 2;
	gait_rolling.slip[Rs].hip_n_now.y = (vmc[0].epos_n.y + vmc[1].epos_n.y) / 2;
	gait_rolling.slip[Rs].hip_n_now.z = (vmc[0].epos_n.z + vmc[1].epos_n.z) / 2;

	gait_rolling.slip[Ls].hip_nb_now.x = (vmc[2].epos_n_b.x + vmc[3].epos_n_b.x) / 2;
	gait_rolling.slip[Ls].hip_nb_now.y = (vmc[2].epos_n_b.y + vmc[3].epos_n_b.y) / 2;
	gait_rolling.slip[Ls].hip_nb_now.z = (vmc[2].epos_n_b.z + vmc[3].epos_n_b.z) / 2;

	gait_rolling.slip[Rs].hip_nb_now.x = (vmc[0].epos_n_b.x + vmc[1].epos_n_b.x) / 2;
	gait_rolling.slip[Rs].hip_nb_now.y = (vmc[0].epos_n_b.y + vmc[1].epos_n_b.y) / 2;
	gait_rolling.slip[Rs].hip_nb_now.z = (vmc[0].epos_n_b.z + vmc[1].epos_n_b.z) / 2;

	gait_rolling.slip[Ls].dhip_n_now.x = (vmc[2].spd_n.x + vmc[3].spd_n.x) / 2;
	gait_rolling.slip[Ls].dhip_n_now.y = (vmc[2].spd_n.y + vmc[3].spd_n.y) / 2;
	gait_rolling.slip[Ls].dhip_n_now.z = (vmc[2].spd_n.z + vmc[3].spd_n.z) / 2;

	gait_rolling.slip[Rs].dhip_n_now.x = (vmc[0].spd_n.x + vmc[1].spd_n.x) / 2;
	gait_rolling.slip[Rs].dhip_n_now.y = (vmc[0].spd_n.y + vmc[1].spd_n.y) / 2;
	gait_rolling.slip[Rs].dhip_n_now.z = (vmc[0].spd_n.z + vmc[1].spd_n.z) / 2;

	gait_rolling.slip[Ls].touch = vmc[2].is_touch && vmc[3].is_touch;
	gait_rolling.slip[Rs].touch = vmc[0].is_touch && vmc[1].is_touch;

	for (i = 0; i < 2; i++) {
		gait_rolling.slip[i].dhip_n_exp.x = gait_rolling.slip_param.vd;
	}
}


void rolling_output_process(float dt)//控制输出
{
	vmc[0].ground = vmc[1].ground = gait_rolling.slip[Rs].ground;
	vmc[2].ground = vmc[3].ground = gait_rolling.slip[Ls].ground;

	if (gait_rolling.slip[Rs].ground) {
		robotwb.Leg[0].F_n_ff.x = gait_rolling.slip[Rs].Fx / 2;
		robotwb.Leg[1].F_n_ff.x = gait_rolling.slip[Rs].Fx / 2;

		robotwb.Leg[0].F_n_ff.z = gait_rolling.slip[Rs].Fz / 2;
		robotwb.Leg[1].F_n_ff.z = gait_rolling.slip[Rs].Fz / 2;

		stand_force_enable_flag[0] = 1;
		stand_force_enable_flag[1] = 1;
	}
	else {
		robotwb.Leg[0].F_n_ff.x = 0;
		robotwb.Leg[1].F_n_ff.x = 0;

		robotwb.Leg[0].F_n_ff.z = 0;
		robotwb.Leg[1].F_n_ff.z = 0;

		stand_force_enable_flag[0] = 0;
		stand_force_enable_flag[1] = 0;
	}

	if (gait_rolling.slip[Ls].ground) {
		robotwb.Leg[2].F_n_ff.x = gait_rolling.slip[Ls].Fx / 2;
		robotwb.Leg[3].F_n_ff.x = gait_rolling.slip[Ls].Fx / 2;

		robotwb.Leg[2].F_n_ff.z = gait_rolling.slip[Ls].Fz / 2;
		robotwb.Leg[3].F_n_ff.z = gait_rolling.slip[Ls].Fz / 2;

		stand_force_enable_flag[2] = 1;
		stand_force_enable_flag[3] = 1;
	}
	else {
		robotwb.Leg[2].F_n_ff.x = 0;
		robotwb.Leg[3].F_n_ff.x = 0;

		robotwb.Leg[2].F_n_ff.z = 0;
		robotwb.Leg[3].F_n_ff.z = 0;

		stand_force_enable_flag[2] = 0;
		stand_force_enable_flag[3] = 0;
	}
}


void rolling_force_impluse(Gait_Bound_IMP *slip, float dt)//计算前馈脉冲
{
	slip->alfa_Fz = gait_rolling.slip_param.mess*9.81*slip->T / (2 * gait_rolling.slip_param.c*slip->T_st_now);
	slip->alfa_tauP = gait_rolling.slip_param.tauP;//速度 支撑时间相关

	slip->s = slip->t / slip->T_st_now;
	slip->s = limitw(slip->s, 0, 1);

	slip->Fz_imp = cal_Impluse_Fz(slip->alfa_Fz, slip->s, gait_rolling.slip_param.s_peak_Fz)*slip->k_jump;
	slip->TauP_imp = cal_Impluse_tauP(slip->alfa_tauP, slip->s, slip->phase);
	slip->g_st = cal_Impluse_gs(slip->s);
	slip->g_st = limitw(slip->g_st, 0, 1);

}

void rolling_balance_control(Gait_Bound_IMP *slip, float dt)//支撑平衡控制
{
	float Fz_ff = 0;
	slip->g_st = limitw(slip->g_st, 0, 1);

	Fz_ff = slip->Fz_imp;

	float Fz_hip = 0;//高度
	Fz_hip = slip->g_st*(slip->pid_hip[0] * (slip->hip_n_exp.z - fabs(slip->hip_n_now.z)) - slip->pid_hip[2] * slip->dhip_n_now.z);
	//printf("g_st=%f exp=%f now=%f\n", slip->g_st, slip->hip_n_exp.z, slip->hip_nb_now.z);

	float Fx_v = 0;//速度
	static float Fx_i = 0;
	Fx_i += slip->pid_v[1] * (slip->dhip_n_exp.x - slip->dhip_n_now.x)*dt;
	Fx_i = limitw(Fx_i, -robotwb.max_force.x, robotwb.max_force.x*0.6);
	Fx_v = slip->pid_v[0] * (slip->dhip_n_exp.x - slip->dhip_n_now.x)
		+ Fx_i 
		- slip->pid_hip_x[0] * (slip->hip_n_exp.x - slip->hip_n_now.x);//FF力

	float Fz_r = 0;//姿态
	Fz_r = (1 / slip->hip_n_now.y)*(slip->pid_R[0] * (slip->att_now[ROLr] - slip->att_exp[ROLr]) - slip->pid_R[2] * slip->att_now_rate[ROLr]) / 57.3;
	
	float Fz_rolling = 0;
    Fz_rolling = slip->TauP_imp / slip->hip_n_now.y;
	slip->Fx = Fx_v;

	float ff_g = 0;
	if (gait_rolling.slip[slip->other_phase].ground)
		ff_g = gait_rolling.slip_param.mess*9.8 / 2;
	else
		ff_g = gait_rolling.slip_param.mess*9.8;

	slip->Fz = Fz_ff + Fz_hip + Fz_r + Fz_rolling+ ff_g;
}

float roll_over_q1[2] = { 100,100 };
float roll_over_dq1[2] = { 800,700 };

float roll_over_q2[2] = { 180,180 };
//float roll_over_q2[2] = { 0,0 };
float roll_over_dq2[2] = { 200,300 };

float roll_over_qoff = 15;
void  Gait_Rolling_Update_v1(float dt)//
{
	
	static float time_delay, yaw_force;
	static float timer_task[10];
	static float delay_timer = 0;
	static float time_cnt[3] = { 0 };
	char i, j;
	char temp_flag[4][2] = { 0 };
	char temp_flag_all = 0;

	rolling_sensor_process(dt);
	//LSM

	switch (rolling_lsm)
	{
		case 0:
			if (1) {	
				gait_rolling.slip[rolling_way].t = gait_rolling.slip[rolling_diag_way].t = 0;
				rolling_lsm++;

				gait_rolling.slip[rolling_way].ground = 1;
				gait_rolling.slip[rolling_diag_way].ground = 1;
				for (i = 0; i < 2; i++) {
					gait_rolling.slip[i].Fz_imp = 0;
					gait_rolling.slip[i].TauP_imp = 0;
					gait_rolling.slip[i].g_st =1;
				}
			}
				
		break;
		case 1://下蹲
			gait_rolling.slip[rolling_way].hip_n_exp.z	    -= 0.25*dt;
			gait_rolling.slip[rolling_diag_way].hip_n_exp.z -= 0.45*dt;
			if (fabs(gait_rolling.slip[rolling_diag_way].hip_nb_now.z) < 0.46*fabs(MAX_Z)) {
				printf("Rolling couch done\n");
				rolling_lsm++;
				delay_timer = 0;
			}
		break;
		case 2://delay
			delay_timer += dt;
			if (delay_timer > 0.05) {
				delay_timer = 0;
				rolling_lsm++;
			}
		break;
		case 3://侧踢
			delay_timer += dt;

			gait_rolling.slip[rolling_way].t += dt;
			gait_rolling.slip[rolling_diag_way].t += dt;

			if(rolling_way==Ls)
				gait_rolling.slip[Ls].att_exp[ROLr] = gait_rolling.slip[Rs].att_exp[ROLr] = gait_rolling.slip[rolling_way].g_st * 15;
			else
				gait_rolling.slip[Ls].att_exp[ROLr] = gait_rolling.slip[Rs].att_exp[ROLr] = gait_rolling.slip[rolling_way].g_st * -15;
			for (i = 0; i < 2; i++) {
				if (i == rolling_way) {
					gait_rolling.slip[i].hip_n_exp.z= 0.8*fabs(MAX_Z);
					
					rolling_force_impluse(&gait_rolling.slip[i], dt);//脉冲规划
				}
				else {
					gait_rolling.slip[i].pid_R[0] = 0;
					gait_rolling.slip[i].g_st = 1;
				}			
			}

			//printf("%d\n", gait_rolling.slip[rolling_way].touch);
			if (!gait_rolling.slip[rolling_way].touch)
			{
				time_cnt[0] += dt;
			}
			else
				time_cnt[0] = 0;

			if ((time_cnt[0]>0.05&&gait_rolling.slip[rolling_way].ground)
			||(fabs(gait_rolling.slip[rolling_way].hip_nb_now.z)>fabs(gait_rolling.slip[rolling_way].hip_n_exp.z)))
			
			
			{
				gait_rolling.slip[rolling_way].ground = 0;
				printf("Side lift\n");
			}
			//printf("gait_rolling.slip[rolling_way].att_now[ROLr]=%f\n", gait_rolling.slip[rolling_way].att_now[ROLr]);
			if (!gait_rolling.slip[rolling_way].ground&&fabs(gait_rolling.slip[rolling_way].att_now[ROLr])>70) {
				printf("Rolling.......!\n");
				gait_rolling.slip[rolling_way].ground = 0;
				gait_rolling.slip[rolling_diag_way].ground = 0;
				rolling_lsm++;
				delay_timer = 0;
				for (j = 0; j < 4; j++) {
					reset_tar_pos(j);
					robotwb.Leg[j].tar_sita[0] = vmc[j].tar_sita1 = vmc[j].sita1;
					robotwb.Leg[j].tar_sita[1] = vmc[j].tar_sita2 = vmc[j].sita2;
				}
			}
			else if (delay_timer > 2) {
				printf("Rolling fail!\n");
				gait_rolling.slip[rolling_way].ground = 0;
				gait_rolling.slip[rolling_diag_way].ground = 0;
				rolling_lsm=6;//去最后

				if (fabs(robotwb.IMU_now_o.pitch) > 110) {
					printf("Facing Down!!\n");
					vmc_all.side_flip = 1;
				}
				else {
					printf("Facing up!!\n");
					vmc_all.side_flip = 0;

				}
				delay_timer = 0;
				for (j = 0; j < 4; j++) {
					reset_tar_pos(j);
					robotwb.Leg[j].tar_sita[0] = vmc[j].tar_sita1 = vmc[j].sita1;
					robotwb.Leg[j].tar_sita[1] = vmc[j].tar_sita2 = vmc[j].sita2;
				}
			}
		break;
		case 4://关节反转
		   //蹬腿侧
			if (rolling_way == Ls) {// 
#if 0
				temp_flag_all =move_joint_to_pos_sel(&vmc[2], 0, -roll_over_q1[0], roll_over_dq1[0], 1, 0,dt);
				temp_flag_all *=move_joint_to_pos_sel(&vmc[2], 1, 180- roll_over_q1[0], roll_over_dq1[0], 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[3], 0, roll_over_q1[0], roll_over_dq1[0], 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[3], 1, 180+ roll_over_q1[0], roll_over_dq1[0], 1, 0, dt);

				temp_flag_all *= move_joint_to_pos_sel(&vmc[0], 0, -roll_over_q1[1], roll_over_dq1[1], 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[0], 1, 180 - roll_over_q1[1], roll_over_dq1[1], 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[1], 0, roll_over_q1[1], roll_over_dq1[1], 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[1], 1, 180 + roll_over_q1[1], roll_over_dq1[1], 1, 0, dt);
#else
				temp_flag_all = move_joint_to_pos_sel(&vmc[2], 0, -roll_over_q1[1], roll_over_dq1[1] * 0.7, 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[2], 1, 180 - roll_over_q1[1], roll_over_dq1[1], 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[3], 0, roll_over_q1[1], roll_over_dq1[1] * 0.7, 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[3], 1, 180 + roll_over_q1[1], roll_over_dq1[1], 1, 0, dt);

				temp_flag_all *= move_joint_to_pos_sel(&vmc[0], 0, -roll_over_q1[0], roll_over_dq1[0] * 0.7, 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[0], 1, 180 - roll_over_q1[0], roll_over_dq1[0], 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[1], 0, roll_over_q1[0], roll_over_dq1[0] * 0.7, 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[1], 1, 180 + roll_over_q1[0], roll_over_dq1[0], 1, 0, dt);
#endif
			}else{
				//printf("ss\n"); not good
				temp_flag_all = move_joint_to_pos_sel(&vmc[2], 0, -roll_over_q1[1], roll_over_dq1[1] * 0.7, 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[2], 1, 180 - roll_over_q1[1], roll_over_dq1[1], 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[3], 0, roll_over_q1[1], roll_over_dq1[1] * 0.7, 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[3], 1, 180 + roll_over_q1[1], roll_over_dq1[1], 1, 0, dt);

				temp_flag_all *= move_joint_to_pos_sel(&vmc[0], 0, -roll_over_q1[0], roll_over_dq1[0] * 0.7, 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[0], 1, 180 - roll_over_q1[0], roll_over_dq1[0], 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[1], 0, roll_over_q1[0], roll_over_dq1[0]*0.7, 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[1], 1, 180 + roll_over_q1[0], roll_over_dq1[0], 1, 0, dt);
			}

			if (fabs(gait_rolling.slip[rolling_way].att_now[ROLr]) < 25&&1) {
				rolling_lsm++;
				//printf("Reset normal q\n");
				for (j = 0; j < 4; j++) {
					reset_tar_pos(j);
					robotwb.Leg[j].tar_sita[0] = vmc[j].tar_sita1 = vmc[j].sita1;
					robotwb.Leg[j].tar_sita[1] = vmc[j].tar_sita2 = vmc[j].sita2;
				}
			}
		break;
		case 5://关节复位

			if (rolling_way == Ls) {// 
				temp_flag_all = move_joint_to_pos_sel(&vmc[2], 0, -roll_over_qoff - roll_over_q2[1], roll_over_dq2[1] * 0.7, 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[2], 1, roll_over_qoff + 180 - roll_over_q2[1], roll_over_dq2[1], 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[3], 0, -roll_over_qoff - roll_over_q2[1], roll_over_dq2[1] * 0.7, 1, 1, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[3], 1, roll_over_qoff + 180 + roll_over_q2[1], roll_over_dq2[1], 1, 1, dt);

				temp_flag_all *= move_joint_to_pos_sel(&vmc[0], 0, -roll_over_qoff - roll_over_q2[0], roll_over_dq2[0] * 0.7, 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[0], 1, roll_over_qoff + 180 - roll_over_q2[0], roll_over_dq2[0], 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[1], 0, -roll_over_qoff + roll_over_q2[0], roll_over_dq2[0] * 0.7, 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[1], 1, roll_over_qoff + 180 + roll_over_q2[0], roll_over_dq2[0], 1, 0, dt);
			}
			else {
				//not good
				temp_flag_all = move_joint_to_pos_sel(&vmc[2], 0, -roll_over_qoff -roll_over_q2[1], roll_over_dq2[0] * 0.7, 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[2], 1, roll_over_qoff +180 - roll_over_q2[1], roll_over_dq2[0], 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[3], 0, -roll_over_qoff -roll_over_q2[1], roll_over_dq2[0] * 0.7, 1, 1, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[3], 1, roll_over_qoff +180 + roll_over_q2[1], roll_over_dq2[0], 1, 1, dt);

				temp_flag_all *= move_joint_to_pos_sel(&vmc[0], 0, -roll_over_qoff -roll_over_q2[0], roll_over_dq2[1] * 0.7, 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[0], 1, roll_over_qoff + 180 - roll_over_q2[0], roll_over_dq2[1], 1, 0, dt);
				//not good 多转一圈
				//printf("ss\n");
				temp_flag_all *= move_joint_to_pos_sel(&vmc[1], 0, -roll_over_qoff + roll_over_q2[0], roll_over_dq2[1]*0.7, 1, 0, dt);
				temp_flag_all *= move_joint_to_pos_sel(&vmc[1], 1, roll_over_qoff + 180 + roll_over_q2[0], roll_over_dq2[1], 1, 0, dt);
			}

			if ( temp_flag_all)
			{
				
				if (fabs(gait_rolling.slip[rolling_way].att_now[PITr]) > 110) {
					printf("Robot Flip succuess!\n");
					if (fabs(robotwb.IMU_now_o.pitch) > 110) {
						printf("Facing Down!!\n");
						vmc_all.side_flip = 1;
					}
					else {
						printf("Facing up!!\n");
						vmc_all.side_flip = 0;
					}
					vmc_all.side_flip_done = 1;
				}
				else
				{
					printf("Robot Flip Fail!\n");
					rolling_lsm++;
				}

				for (j = 0; j < 4; j++) {
					reset_tar_pos(j);
					robotwb.Leg[j].tar_sita[0] = vmc[j].tar_sita1 = vmc[j].sita1;
					robotwb.Leg[j].tar_sita[1] = vmc[j].tar_sita2 = vmc[j].sita2;
				}
				
			}
			break;
			case 6://失败复位
				if (rolling_way == Ls) {// 
					temp_flag_all = move_joint_to_pos_sel(&vmc[2], 0, -35 , roll_over_dq2[0], 1, 0, dt);
					temp_flag_all *= move_joint_to_pos_sel(&vmc[2], 1, 35 + 180 , roll_over_dq2[0], 1, 0, dt);
					temp_flag_all *= move_joint_to_pos_sel(&vmc[3], 0, -35, roll_over_dq2[0], 1, 0, dt);
					temp_flag_all *= move_joint_to_pos_sel(&vmc[3], 1, 35 + 180 , roll_over_dq2[0], 1, 0, dt);

					temp_flag_all *= move_joint_to_pos_sel(&vmc[0], 0, -35 , roll_over_dq2[1], 1, 0, dt);
					temp_flag_all *= move_joint_to_pos_sel(&vmc[0], 1, 35 + 180 , roll_over_dq2[1], 1, 0, dt);
					temp_flag_all *= move_joint_to_pos_sel(&vmc[1], 0, -35 , roll_over_dq2[1], 1, 0, dt);
					temp_flag_all *= move_joint_to_pos_sel(&vmc[1], 1, 35 + 180, roll_over_dq2[1], 1, 0, dt);
				}
				else {
					temp_flag_all = move_joint_to_pos_sel(&vmc[2], 0, -35 , roll_over_dq2[1], 1, 0, dt);
					temp_flag_all *= move_joint_to_pos_sel(&vmc[2], 1, 35 + 180, roll_over_dq2[1], 1, 0, dt);
					temp_flag_all *= move_joint_to_pos_sel(&vmc[3], 0, -35 , roll_over_dq2[1], 1, 1, dt);
					temp_flag_all *= move_joint_to_pos_sel(&vmc[3], 1, 35 + 180 , roll_over_dq2[1], 1, 1, dt);

					temp_flag_all *= move_joint_to_pos_sel(&vmc[0], 0, -35 , roll_over_dq2[0], 1, 0, dt);
					temp_flag_all *= move_joint_to_pos_sel(&vmc[0], 1, 35 + 180, roll_over_dq2[0], 1, 0, dt);
					temp_flag_all *= move_joint_to_pos_sel(&vmc[1], 0, -35 , roll_over_dq2[0], 1, 0, dt);
					temp_flag_all *= move_joint_to_pos_sel(&vmc[1], 1, 35 + 180, roll_over_dq2[0], 1, 0, dt);
				}

				if (temp_flag_all)
				{

					if (fabs(gait_rolling.slip[rolling_way].att_now[PITr]) <30) {
						printf("Robot Reset Done!\n");
						vmc_all.side_flip_done = 1;
					}
				
					for (j = 0; j < 4; j++) {
						reset_tar_pos(j);
						robotwb.Leg[j].tar_sita[0] = vmc[j].tar_sita1 = vmc[j].sita1;
						robotwb.Leg[j].tar_sita[1] = vmc[j].tar_sita2 = vmc[j].sita2;
					}

				}
			break;
	}
	//printf("[ROLr]=%f PITr=%f\n", gait_rolling.slip[rolling_way].att_now[ROLr], gait_rolling.slip[rolling_way].att_now[PITr]);
	rolling_balance_control(&gait_rolling.slip[rolling_way], dt);//稳定控制
	rolling_balance_control(&gait_rolling.slip[rolling_diag_way], dt);

	rolling_output_process(dt);

	for (i = 0; i < 10; i++)
		timer_task[i] += dt;
}

